﻿/*
 * Agent.cs
 * RVO2 Library C#
 *
 * Copyright 2008 University of North Carolina at Chapel Hill
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
 * Dept. of Computer Science
 * 201 S. Columbia St.
 * Frederick P. Brooks, Jr. Computer Science Bldg.
 * Chapel Hill, N.C. 27599-3175
 * United States of America
 *
 * <http://gamma.cs.unc.edu/RVO2/>
 */

using System;
using System.Collections.Generic;
using UnityEngine;

namespace Core.RVO
{
    /**
     * <summary>Defines an agent in the simulation.</summary>
     */
    public class Agent : Core.Component, IAwake
    {
        //private readonly IList<(float, Agent)> agentNeighbors111 = new List<(float, Agent)>();
        private readonly IList<(float distSQ, Agent agent)> agentNeighbors = new List<(float, Agent)>();
        private readonly IList<(float distSQ, Obstacle obstacle)> obstacleNeighbors = new List<(float, Obstacle)>();
        private readonly IList<Line> orcaLines = new List<Line>();

        public int Id { get; private set; }
        public int MaxNeighbors { get; private set; }
        public float MaxSpeed { get; private set; }
        public float NeighborDis { get; private set; }
        public Vector2 Position { get; private set; }
        public Vector2 PrefVelocity { get; private set; }
        public float Radius { get; private set; }
        public float TimeHorizon { get; private set; }
        public float TimeHorizonObst { get; private set; }
        public Vector2 Velocity { get; private set; }

        private Vector2 newVelocity;
        private RVOSystem sys;

        public void Awake()
        {
            sys = Entity as RVOSystem;
        }

        public override void Dispose()
        {
            if (IsDisposed) return;

            SetData(0, Vector2.zero, 0.0f, 0, 0.0f, 0.0f, 0.0f, 0.0f, Vector2.zero);

            base.Dispose();
        }

        public void SetData(int id, Vector2 position, float neighbor_dis,
            int max_neighbors, float time_horizon, float time_horizon_obst,
            float radius, float max_speed, Vector2 velocity)
        {
            Id = id;
            Position = position;
            NeighborDis = neighbor_dis;
            MaxNeighbors = max_neighbors;
            TimeHorizon = time_horizon;
            TimeHorizonObst = time_horizon_obst;
            Radius = radius;
            MaxSpeed = max_speed;
            Velocity = velocity;
        }

        /**
         * <summary>Computes the neighbors of this agent.</summary>
         */
        public void ComputeNeighbors()
        {
            obstacleNeighbors.Clear();

            float rangeSq = (TimeHorizonObst * MaxSpeed + Radius).Square();
            sys.KdTree.ComputeObstacleNeighbors(this, rangeSq);

            agentNeighbors.Clear();

            if (MaxNeighbors > 0)
            {
                rangeSq = NeighborDis.Square();
                sys.KdTree.ComputeAgentNeighbors(this, ref rangeSq);
            }
        }

        /**
         * <summary>Computes the new velocity of this agent.</summary>
         */
        public void ComputeNewVelocity()
        {
            orcaLines.Clear();

            float invTimeHorizonObst = 1.0f / TimeHorizonObst;

            /* Create obstacle ORCA lines. */
            for (int i = 0; i < obstacleNeighbors.Count; ++i)
            {
                Obstacle obstacle1 = obstacleNeighbors[i].obstacle;
                Obstacle obstacle2 = obstacle1.Next;

                Vector2 relativePosition1 = obstacle1.Pos - Position;
                Vector2 relativePosition2 = obstacle2.Pos - Position;

                /*
                 * Check if velocity obstacle of obstacle is already taken care
                 * of by previously constructed obstacle ORCA lines.
                 */
                bool alreadyCovered = false;

                for (int j = 0; j < orcaLines.Count; ++j)
                {
                    var orcaLine = orcaLines[j];
                    if (RVOMath.Det(invTimeHorizonObst * relativePosition1 - orcaLine.Pos, orcaLine.Dir) -
                        invTimeHorizonObst * Radius >= -RVOMath.kEpsilon
                        && RVOMath.Det(invTimeHorizonObst * relativePosition2 - orcaLine.Pos, orcaLine.Dir) -
                        invTimeHorizonObst * Radius >= -RVOMath.kEpsilon)
                    {
                        alreadyCovered = true;

                        break;
                    }
                }

                if (alreadyCovered)
                {
                    continue;
                }

                /* Not yet covered. Check for collisions. */
                float distSq1 = relativePosition1.sqrMagnitude;
                float distSq2 = relativePosition2.sqrMagnitude;

                float radiusSq = Radius * Radius;

                Vector2 obstacleVector = obstacle2.Pos - obstacle1.Pos;
                float s = Vector2.Dot(-relativePosition1, obstacleVector) / obstacleVector.sqrMagnitude;
                float distSqLine = (-relativePosition1 - s * obstacleVector).sqrMagnitude;

                Line line;

                switch (s)
                {
                    case < 0.0f when distSq1 <= radiusSq:
                    {
                        /* Collision with left vertex. Ignore if non-convex. */
                        if (obstacle1.Convex)
                        {
                            line.Pos = new Vector2(0.0f, 0.0f);
                            line.Dir = new Vector2(-relativePosition1.y, relativePosition1.x).normalized;
                            orcaLines.Add(line);
                        }

                        continue;
                    }
                    case > 1.0f when distSq2 <= radiusSq:
                    {
                        /*
                         * Collision with right vertex. Ignore if non-convex or if
                         * it will be taken care of by neighboring obstacle.
                         */
                        if (obstacle2.Convex && RVOMath.Det(relativePosition2, obstacle2.Dir) >= 0.0f)
                        {
                            line.Pos = new Vector2(0.0f, 0.0f);
                            line.Dir = new Vector2(-relativePosition2.y, relativePosition2.x).normalized;
                            orcaLines.Add(line);
                        }

                        continue;
                    }
                    case >= 0.0f and < 1.0f when distSqLine <= radiusSq:
                    {
                        /* Collision with obstacle segment. */
                        line.Pos = new Vector2(0.0f, 0.0f);
                        line.Dir = -obstacle1.Dir;
                        orcaLines.Add(line);

                        continue;
                    }
                }

                /*
                 * No collision. Compute legs. When obliquely viewed, both legs
                 * can come from a single vertex. Legs extend cut-off line when
                 * non-convex vertex.
                 */

                Vector2 leftLegDirection, rightLegDirection;

                if (s < 0.0f && distSqLine <= radiusSq)
                {
                    /*
                     * Obstacle viewed obliquely so that left vertex
                     * defines velocity obstacle.
                     */
                    if (!obstacle1.Convex)
                    {
                        /* Ignore obstacle. */
                        continue;
                    }

                    obstacle2 = obstacle1;

                    float leg1 = Mathf.Sqrt(distSq1 - radiusSq);
                    leftLegDirection = new Vector2(relativePosition1.x * leg1 - relativePosition1.y * Radius,
                        relativePosition1.x * Radius + relativePosition1.y * leg1) / distSq1;
                    rightLegDirection = new Vector2(relativePosition1.x * leg1 + relativePosition1.y * Radius,
                        -relativePosition1.x * Radius + relativePosition1.y * leg1) / distSq1;
                }
                else if (s > 1.0f && distSqLine <= radiusSq)
                {
                    /*
                     * Obstacle viewed obliquely so that
                     * right vertex defines velocity obstacle.
                     */
                    if (!obstacle2.Convex)
                    {
                        /* Ignore obstacle. */
                        continue;
                    }

                    obstacle1 = obstacle2;

                    float leg2 = Mathf.Sqrt(distSq2 - radiusSq);
                    leftLegDirection = new Vector2(relativePosition2.x * leg2 - relativePosition2.y * Radius,
                        relativePosition2.x * Radius + relativePosition2.y * leg2) / distSq2;
                    rightLegDirection = new Vector2(relativePosition2.x * leg2 + relativePosition2.y * Radius,
                        -relativePosition2.x * Radius + relativePosition2.y * leg2) / distSq2;
                }
                else
                {
                    /* Usual situation. */
                    if (obstacle1.Convex)
                    {
                        float leg1 = Mathf.Sqrt(distSq1 - radiusSq);
                        leftLegDirection = new Vector2(relativePosition1.x * leg1 - relativePosition1.y * Radius,
                            relativePosition1.x * Radius + relativePosition1.y * leg1) / distSq1;
                    }
                    else
                    {
                        /* Left vertex non-convex; left leg extends cut-off line. */
                        leftLegDirection = -obstacle1.Dir;
                    }

                    if (obstacle2.Convex)
                    {
                        float leg2 = Mathf.Sqrt(distSq2 - radiusSq);
                        rightLegDirection = new Vector2(relativePosition2.x * leg2 + relativePosition2.y * Radius,
                            -relativePosition2.x * Radius + relativePosition2.y * leg2) / distSq2;
                    }
                    else
                    {
                        /* Right vertex non-convex; right leg extends cut-off line. */
                        rightLegDirection = obstacle1.Dir;
                    }
                }

                /*
                 * Legs can never point into neighboring edge when convex
                 * vertex, take cutoff-line of neighboring edge instead. If
                 * velocity projected on "foreign" leg, no constraint is added.
                 */

                Obstacle leftNeighbor = obstacle1.Prev;

                bool isLeftLegForeign = false;
                bool isRightLegForeign = false;

                if (obstacle1.Convex && RVOMath.Det(leftLegDirection, -leftNeighbor.Dir) >= 0.0f)
                {
                    /* Left leg points into obstacle. */
                    leftLegDirection = -leftNeighbor.Dir;
                    isLeftLegForeign = true;
                }

                if (obstacle2.Convex && RVOMath.Det(rightLegDirection, obstacle2.Dir) <= 0.0f)
                {
                    /* Right leg points into obstacle. */
                    rightLegDirection = obstacle2.Dir;
                    isRightLegForeign = true;
                }

                /* Compute cut-off centers. */
                Vector2 leftCutOff = invTimeHorizonObst * (obstacle1.Pos - Position);
                Vector2 rightCutOff = invTimeHorizonObst * (obstacle2.Pos - Position);
                Vector2 cutOffVector = rightCutOff - leftCutOff;

                /* Project current velocity on velocity obstacle. */

                /* Check if current velocity is projected on cutoff circles. */
                float t = obstacle1 == obstacle2
                    ? 0.5f
                    : Vector2.Dot(Velocity - leftCutOff, cutOffVector) / cutOffVector.sqrMagnitude;
                float tLeft = Vector2.Dot(Velocity - leftCutOff, leftLegDirection);
                float tRight = Vector2.Dot(Velocity - rightCutOff, rightLegDirection);

                if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f))
                {
                    /* Project on left cut-off circle. */
                    Vector2 unitW = (Velocity - leftCutOff).normalized;

                    line.Dir = new Vector2(unitW.y, -unitW.x);
                    line.Pos = leftCutOff + Radius * invTimeHorizonObst * unitW;
                    orcaLines.Add(line);

                    continue;
                }
                else if (t > 1.0f && tRight < 0.0f)
                {
                    /* Project on right cut-off circle. */
                    Vector2 unitW = (Velocity - rightCutOff).normalized;

                    line.Dir = new Vector2(unitW.y, -unitW.x);
                    line.Pos = rightCutOff + Radius * invTimeHorizonObst * unitW;
                    orcaLines.Add(line);

                    continue;
                }

                /*
                 * Project on left leg, right leg, or cut-off line, whichever is
                 * closest to velocity.
                 */
                float distSqCutoff = (t < 0.0f || t > 1.0f || obstacle1 == obstacle2)
                    ? float.PositiveInfinity
                    : (Velocity - (leftCutOff + t * cutOffVector)).sqrMagnitude;
                float distSqLeft = tLeft < 0.0f
                    ? float.PositiveInfinity
                    : (Velocity - (leftCutOff + tLeft * leftLegDirection)).sqrMagnitude;
                float distSqRight = tRight < 0.0f
                    ? float.PositiveInfinity
                    : (Velocity - (rightCutOff + tRight * rightLegDirection)).sqrMagnitude;

                if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
                {
                    /* Project on cut-off line. */
                    line.Dir = -obstacle1.Dir;
                    line.Pos = leftCutOff + Radius * invTimeHorizonObst * new Vector2(-line.Dir.y, line.Dir.x);
                    orcaLines.Add(line);

                    continue;
                }

                if (distSqLeft <= distSqRight)
                {
                    /* Project on left leg. */
                    if (isLeftLegForeign)
                    {
                        continue;
                    }

                    line.Dir = leftLegDirection;
                    line.Pos = leftCutOff + Radius * invTimeHorizonObst * new Vector2(-line.Dir.y, line.Dir.x);
                    orcaLines.Add(line);

                    continue;
                }

                /* Project on right leg. */
                if (isRightLegForeign)
                {
                    continue;
                }

                line.Dir = -rightLegDirection;
                line.Pos = rightCutOff + Radius * invTimeHorizonObst * new Vector2(-line.Dir.y, line.Dir.x);
                orcaLines.Add(line);
            }

            int numObstLines = orcaLines.Count;

            float invTimeHorizon = 1.0f / TimeHorizon;

            /* Create agent ORCA lines. */
            for (int i = 0; i < agentNeighbors.Count; ++i)
            {
                Agent other = agentNeighbors[i].agent;

                Vector2 relativePosition = other.Position - Position;
                Vector2 relativeVelocity = Velocity - other.Velocity;
                float distSq = relativePosition.sqrMagnitude;
                float combinedRadius = Radius + other.Radius;
                float combinedRadiusSq = combinedRadius.Square();

                Line line;
                Vector2 u;

                if (distSq > combinedRadiusSq)
                {
                    /* No collision. */
                    Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;

                    /* Vector from cutoff center to relative velocity. */
                    float wLengthSq = w.sqrMagnitude;
                    float dotProduct1 = Vector2.Dot(w, relativePosition);

                    if (dotProduct1 < 0.0f && dotProduct1.Square() > combinedRadiusSq * wLengthSq)
                    {
                        /* Project on cut-off circle. */
                        float wLength = Mathf.Sqrt(wLengthSq);
                        Vector2 unitW = w / wLength;

                        line.Dir = new Vector2(unitW.y, -unitW.x);
                        u = (combinedRadius * invTimeHorizon - wLength) * unitW;
                    }
                    else
                    {
                        /* Project on legs. */
                        float leg = Mathf.Sqrt(distSq - combinedRadiusSq);

                        if (RVOMath.Det(relativePosition, w) > 0.0f)
                        {
                            /* Project on left leg. */
                            line.Dir = new Vector2(relativePosition.x * leg - relativePosition.y * combinedRadius,
                                relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
                        }
                        else
                        {
                            /* Project on right leg. */
                            line.Dir = -new Vector2(relativePosition.x * leg + relativePosition.y * combinedRadius,
                                -relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
                        }

                        float dotProduct2 = Vector2.Dot(relativeVelocity, line.Dir);
                        u = dotProduct2 * line.Dir - relativeVelocity;
                    }
                }
                else
                {
                    /* Collision. Project on cut-off circle of time timeStep. */
                    float invTimeStep = 1.0f / sys.Workers.TimeStep;

                    /* Vector from cutoff center to relative velocity. */
                    Vector2 w = relativeVelocity - invTimeStep * relativePosition;

                    float wLength = w.magnitude;
                    if (wLength < RVOMath.kEpsilon)
                    {
                        wLength = RVOMath.kEpsilon;
                    }

                    //if (wLength==0)
                    //{
                    //    Debug.LogError("111");
                    //}
                    Vector2 unitW = w / wLength;

                    line.Dir = new Vector2(unitW.y, -unitW.x);
                    u = (combinedRadius * invTimeStep - wLength) * unitW;
                }

                line.Pos = Velocity + 0.5f * u;
                orcaLines.Add(line);
            }

            int lineFail = LinearProgram2(orcaLines, MaxSpeed, PrefVelocity, false, ref newVelocity);

            if (lineFail < orcaLines.Count)
            {
                LinearProgram3(orcaLines, numObstLines, lineFail, MaxSpeed, ref newVelocity);
            }
        }

        /**
         * <summary>Inserts an agent neighbor into the set of neighbors of this
         * agent.</summary>
         *
         * <param name="agent">A pointer to the agent to be inserted.</param>
         * <param name="range_sq">The squared range around this agent.</param>
         */
        public void InsertAgentNeighbor(Agent agent, ref float range_sq)
        {
            if (this != agent)
            {
                float distSq = (Position - agent.Position).sqrMagnitude;

                if (distSq < range_sq)
                {
                    if (agentNeighbors.Count < MaxNeighbors)
                    {
                        agentNeighbors.Add((distSq, agent));
                    }

                    int i = agentNeighbors.Count - 1;

                    while (i != 0 && distSq < agentNeighbors[i - 1].distSQ)
                    {
                        agentNeighbors[i] = agentNeighbors[i - 1];
                        --i;
                    }

                    agentNeighbors[i] = (distSq, agent);

                    if (agentNeighbors.Count == MaxNeighbors)
                    {
                        range_sq = agentNeighbors[^1].distSQ;
                    }
                }
            }
        }

        /**
         * <summary>Inserts a static obstacle neighbor into the set of neighbors
         * of this agent.</summary>
         *
         * <param name="obstacle">The number of the static obstacle to be
         * inserted.</param>
         * <param name="range_sq">The squared range around this agent.</param>
         */
        public void InsertObstacleNeighbor(Obstacle obstacle, float range_sq)
        {
            Obstacle nextObstacle = obstacle.Next;

            float distSq = RVOMath.GetSDFromPointToLineSegment(obstacle.Pos, nextObstacle.Pos, Position);

            if (distSq < range_sq)
            {
                obstacleNeighbors.Add((distSq, obstacle));

                int i = obstacleNeighbors.Count - 1;

                while (i != 0 && distSq < obstacleNeighbors[i - 1].distSQ)
                {
                    obstacleNeighbors[i] = obstacleNeighbors[i - 1];
                    --i;
                }

                obstacleNeighbors[i] = (distSq, obstacle);
            }
        }

        /**
         * <summary>Updates the two-dimensional position and two-dimensional
         * velocity of this agent.</summary>
         */
        public void Update()
        {
            Velocity = newVelocity;
            Position += Velocity * sys.Workers.TimeStep;
        }

        /**
         * <summary>Solves a one-dimensional linear program on a specified line
         * subject to linear constraints defined by lines and a circular
         * constraint.</summary>
         *
         * <returns>True if successful.</returns>
         *
         * <param name="lines">Lines defining the linear constraints.</param>
         * <param name="line_no">The specified line constraint.</param>
         * <param name="radius">The radius of the circular constraint.</param>
         * <param name="opt_velocity">The optimization velocity.</param>
         * <param name="direction_opt">True if the direction should be optimized.
         * </param>
         * <param name="result">A reference to the result of the linear program.
         * </param>
         */
        private bool LinearProgram1(IList<Line> lines, int line_no, float radius, Vector2 opt_velocity,
            bool direction_opt, ref Vector2 result)
        {
            var line = lines[line_no];
            float dotProduct = Vector2.Dot(line.Pos, line.Dir);
            float discriminant = dotProduct.Square() + radius.Square() - line.Pos.sqrMagnitude;

            if (discriminant < 0.0f)
            {
                /* Max speed circle fully invalidates line lineNo. */
                return false;
            }

            float sqrtDiscriminant = Mathf.Sqrt(discriminant);
            float tLeft = -dotProduct - sqrtDiscriminant;
            float tRight = -dotProduct + sqrtDiscriminant;

            for (int i = 0; i < line_no; ++i)
            {
                float denominator = RVOMath.Det(line.Dir, lines[i].Dir);
                float numerator = RVOMath.Det(lines[i].Dir, line.Pos - lines[i].Pos);

                if (Mathf.Abs(denominator) <= RVOMath.kEpsilon)
                {
                    /* Lines lineNo and I are (almost) parallel. */
                    if (numerator < 0.0f)
                    {
                        return false;
                    }

                    continue;
                }

                float t = numerator / denominator;

                if (denominator >= 0.0f)
                {
                    /* Line I bound line lineNo on the right. */
                    tRight = Math.Min(tRight, t);
                }
                else
                {
                    /* Line I bound line lineNo on the left. */
                    tLeft = Math.Max(tLeft, t);
                }

                if (tLeft > tRight)
                {
                    return false;
                }
            }

            if (direction_opt)
            {
                /* Optimize direction. */
                if (Vector2.Dot(opt_velocity, line.Dir) > 0.0f)
                {
                    /* Take right extreme. */
                    result = line.Pos + tRight * line.Dir;
                }
                else
                {
                    /* Take left extreme. */
                    result = line.Pos + tLeft * line.Dir;
                }
            }
            else
            {
                /* Optimize closest point. */
                float t = Vector2.Dot(line.Dir, opt_velocity - line.Pos);

                if (t < tLeft)
                {
                    result = line.Pos + tLeft * line.Dir;
                }
                else if (t > tRight)
                {
                    result = line.Pos + tRight * line.Dir;
                }
                else
                {
                    result = line.Pos + t * line.Dir;
                }
            }

            return true;
        }

        /**
         * <summary>Solves a two-dimensional linear program subject to linear
         * constraints defined by lines and a circular constraint.</summary>
         *
         * <returns>The number of the line it fails on, and the number of lines
         * if successful.</returns>
         *
         * <param name="lines">Lines defining the linear constraints.</param>
         * <param name="radius">The radius of the circular constraint.</param>
         * <param name="opt_velocity">The optimization velocity.</param>
         * <param name="direction_opt">True if the direction should be optimized.
         * </param>
         * <param name="result">A reference to the result of the linear program.
         * </param>
         */
        private int LinearProgram2(IList<Line> lines, float radius, Vector2 opt_velocity, bool direction_opt,
            ref Vector2 result)
        {
            if (direction_opt)
            {
                /*
                 * Optimize direction. Note that the optimization velocity is of
                 * unit length in this case.
                 */
                result = opt_velocity * radius;
            }
            else if (opt_velocity.sqrMagnitude > radius.Square())
            {
                /* Optimize the closest point and outside circle. */
                result = opt_velocity.normalized * radius;
            }
            else
            {
                /* Optimize the closest point and inside circle. */
                result = opt_velocity;
            }

            for (int i = 0; i < lines.Count; ++i)
            {
                if (RVOMath.Det(lines[i].Dir, lines[i].Pos - result) > 0.0f)
                {
                    /* Result does not satisfy constraint i. Compute new optimal result. */
                    Vector2 tempResult = result;
                    if (!LinearProgram1(lines, i, radius, opt_velocity, direction_opt, ref result))
                    {
                        result = tempResult;

                        return i;
                    }
                }
            }

            return lines.Count;
        }

        /**
         * <summary>Solves a two-dimensional linear program subject to linear
         * constraints defined by lines and a circular constraint.</summary>
         *
         * <param name="lines">Lines defining the linear constraints.</param>
         * <param name="num_obst_lines">Count of obstacle lines.</param>
         * <param name="begin_line">The line on which the 2-d linear program
         * failed.</param>
         * <param name="radius">The radius of the circular constraint.</param>
         * <param name="result">A reference to the result of the linear program.
         * </param>
         */
        private void LinearProgram3(IList<Line> lines, int num_obst_lines, int begin_line, float radius,
            ref Vector2 result)
        {
            float distance = 0.0f;

            for (int i = begin_line; i < lines.Count; ++i)
            {
                var lineI = lines[i];

                if (RVOMath.Det(lineI.Dir, lineI.Pos - result) > distance)
                {
                    /* Result does not satisfy constraint of line i. */
                    IList<Line> projLines = new List<Line>();
                    for (int ii = 0; ii < num_obst_lines; ++ii)
                    {
                        projLines.Add(lines[ii]);
                    }

                    for (int j = num_obst_lines; j < i; ++j)
                    {
                        var lineJ = lines[j];

                        Line line;

                        float determinant = RVOMath.Det(lineI.Dir, lineJ.Dir);

                        if (Mathf.Abs(determinant) <= RVOMath.kEpsilon)
                        {
                            /* Line i and line j are parallel. */
                            if (Vector2.Dot(lineI.Dir, lineJ.Dir) > 0.0f)
                            {
                                /* Line i and line j point in the same direction. */
                                continue;
                            }
                            else
                            {
                                /* Line i and line j point in opposite direction. */
                                line.Pos = 0.5f * (lineI.Pos + lineJ.Pos);
                            }
                        }
                        else
                        {
                            line.Pos = lineI.Pos + (RVOMath.Det(lineJ.Dir, lineI.Pos - lineJ.Pos) / determinant) *
                                lineI.Dir;
                        }

                        line.Dir = (lineJ.Dir - lineI.Dir).normalized;
                        projLines.Add(line);
                    }

                    Vector2 tempResult = result;
                    if (LinearProgram2(projLines, radius, new Vector2(-lineI.Dir.y, lineI.Dir.x), true, ref result) <
                        projLines.Count)
                    {
                        /*
                         * This should in principle not happen. The result is by
                         * definition already in the feasible region of this
                         * linear program. If it fails, it is due to small
                         * floating point error, and the current result is kept.
                         */
                        result = tempResult;
                    }

                    distance = RVOMath.Det(lineI.Dir, lineI.Pos - result);
                }
            }
        }
    }
}
